#include "steer.h"
unsigned char steering_control[]={
    0x01, 0x03, 0x00, 0x00, 0x00, 0x01, 0x84, 0x0a,     //读取软件版本号
    0x01, 0x06, 0x00, 0x02, 0x00, 0x00, 0x28, 0x0a,     //设置舵机速度0（最慢）
    0x01, 0x06, 0x00, 0x02, 0x00, 0x07, 0x69, 0xC8,     //设置舵机速度0（最快） 
    0x01, 0x06, 0x00, 0x06, 0x01, 0xf4, 0x69, 0xdc,     //舵机转到0度（位置值500） 
    0x01, 0x06, 0x00, 0x06, 0x05, 0xdc, 0x6b, 0x02,     //舵机转到90度（位置值1500） 
    0x01, 0x06, 0x00, 0x06, 0x09, 0xc4, 0x6e, 0x08,     //舵机转到180度（位置值2500）   
    0x01, 0x03, 0x00, 0x06, 0x00, 0x01, 0x64, 0x0b,     //读舵机的位置 
    0x01, 0x03, 0x00, 0x0a, 0x00, 0x01, 0xa4, 0x08,     //读所有舵机运动状态 
    0x01, 0x03, 0x00, 0x0b, 0x00, 0x01, 0xf5, 0xc8,     //读按钮自上次读取后是否被按下过    
    0x01, 0x06, 0x00, 0x0c, 0x00, 0x01, 0x88, 0x09,     //设置用户LED灯开   
    0x01, 0x06, 0x00, 0x0c, 0x00, 0x00, 0x49, 0xc9,     //设置用户LED灯关   
};

/***********************************************
uart_open:
func: 配置串口
param: 要配置的端口号0~3(本项目使用的是端口2)
return: 成功返回设备文件的文件描述符, 错误返回-1
***********************************************/
int uart_open (int com)
{
	int fd;
	char dev_uart[128];	
	sprintf (dev_uart, "/dev/ttyAMA%d", com);
	fd = open (dev_uart, O_RDWR | O_NOCTTY | O_NDELAY);
	if (fd < 0) {
		perror ("open serial port failed!");
		return -1;
	}
	//修改文件描述符
	if (0 > fcntl (fd, F_SETFL, 0)) {
		perror ("fcntl error!");
		return -1;	
	}
	//检查标准输入（通常是键盘）是否关联到一个终端设备
	if (isatty (STDIN_FILENO) == 0) {
		printf ("standard input is not a terminal device\n");   
	}
	return fd;
}

/***********************************************
uart_setting:
func: 设置对应串口的属性信息
param: fd, 对应的已打开的串口设备的文件描述符
	   uart, uart核心数据结构的变量
return: 成功返回0, 错误返回-1
***********************************************/
int uart_setting (int fd, struct uart_desc uart)
{
	struct termios oldtio, newtio;

	//将设备的属性保存到oldtio中
	if (0 != tcgetattr (fd, &oldtio)) {
		perror ("tcgetattr");
		return -1;
	}

	//将newtio的所有字节设置为0
	bzero (&newtio, sizeof(newtio));
	//硬件流控不选
	//CLOCAL：用于忽略调制解调器（modem）的状态线
	//CREAD：用于启用字符接收
	newtio.c_cflag |= CLOCAL | CREAD;

	//清除CSZIE字段
	newtio.c_cflag &= ~CSIZE;
	
	//设置数据位
	switch (uart.nbits)
	{
		case 5:
			newtio.c_cflag |= CS5;
			break;
		case 6:
			newtio.c_cflag |= CS6;
			break;
		case 7:
			newtio.c_cflag |= CS7;
			break;
		case 8:
			newtio.c_cflag |= CS8;
			break;
	}

	//设置校验方式
	switch (uart.event) {
		case ODD:
			newtio.c_cflag |= PARENB | PARODD;
			newtio.c_cflag |= (INPCK | ISTRIP);
			break;
		case EVEN:
			newtio.c_cflag |= PARENB;
			newtio.c_cflag |= (INPCK | ISTRIP);
			newtio.c_cflag &=  ~(PARENB);
			break;
		case NONE:
			newtio.c_cflag &=  ~(PARENB);
			break;
		case EVENT_MAX:
			printf ("check bit setting error");
			return -1;
	}

	//设置波特率
	switch (uart.baudrate) {
		case 2400:
			cfsetispeed (&newtio, B2400);
			cfsetospeed (&newtio, B2400);
			break;
		case 9600:
			cfsetispeed (&newtio, B9600);
			cfsetospeed (&newtio, B9600);
			break;
		case 115200:
			cfsetispeed (&newtio, B115200);
			cfsetospeed (&newtio, B115200);
			break;
	}

	//设置停止位
	if (uart.stopbits == 1) {
		newtio.c_cflag &= ~CSTOPB;
	} else if (uart.stopbits == 2) {
		newtio.c_cflag |= CSTOPB;
	}
	
	//轮询读：
	//用于指定非规范模式下读取输入字符时的超时时间（以十分之一秒为单位）。
	//如果设置为0，则读取操作将一直等待，直到达到VMIN指定的字符数或遇到EOF字符（通常是Ctrl+D）。
	newtio.c_cc[VTIME] = 0;
	//用于指定非规范模式下读取操作之前必须接收到的最小字符数。
	//如果设置为0，并且VTIME也设置为0，则读取操作将立即返回，无论是否读取到任何字符。
	newtio.c_cc[VMIN] = 0;
	//用于清空输入或输出队列。
	tcflush (fd, TCIFLUSH);

	//将 newtio 结构体中定义的属性立即应用到与文件描述符 fd 关联的串口设备上
	if (0 != (tcsetattr (fd, TCSANOW, &newtio))) {
		perror ("tcsetattr");
		return -1;
	}
	return 0;
}

/***********************************************
steering_ctrl:
func: 将命令写入到打开的串口设备的文件描述符fd中
param: fd, 对应的串口文件描述符
	   buf, 存放命令的数组
	   cmd, 要访问的数组的下标
return: 成功返回回填参数个数
***********************************************/
int steering_ctrl (int fd, char *outdata, enum steering_cmd cmd)
{
	int ret;
	//printf("enter steering_ctrl !\n");
	switch (cmd) 
	{
		case VERSION:
		ret = write (fd, &steering_control[0*8], 8);
			break;
		case SPEED0:
		ret = write (fd, &steering_control[1*8], 8);
			break;
		case SPEED7:
		ret = write (fd, &steering_control[2*8], 8);
			break;
		case DEGREE0:
		ret = write (fd, &steering_control[3*8], 8);
			break;
		case DEGREE90:
		ret = write (fd, &steering_control[4*8], 8);
			break;
		case DEGREE180:
		ret = write (fd, &steering_control[5*8], 8);
			break;
		case POS_READ:
		ret = write (fd, &steering_control[6*8], 8);
			break;
		case STAT_READ:
		ret = write (fd, &steering_control[7*8], 8);
			break;
		case BUT_READ:
		ret = write (fd, &steering_control[8*8], 8);
			break;
		case  LED_ON:
		ret = write (fd, &steering_control[9*8], 8);
			break;
		case LED_OFF:
		ret = write (fd, &steering_control[10*8], 8);
			break;
		default:
			break;
	}
	//printf("enter steering_ctrl1 !\n");
	if (ret == 8) {
		usleep (5000);
		ret = read (fd, outdata, 8);
		//printf("enter steering_ctrl2 !\n");
		if (ret != 8) {
			printf ("read faild!\n");
		}
	}
	//printf("enter steering_ctrl2 !\n");
	return ret;
}

/***************************************************
功能：串口初始化
参数：
	 端口、波特率、数据位、停止位、校验方式

***************************************************/
void uart_init (void)
{
	struct uart_desc uart;
	//int fd = uart_fd; 
	//端口设置为COM2
	uart.port = COMPORT2;
	//打开串口
	if (0 > (uart_fd = uart_open (uart.port))) {
		perror ("uart_open error!");
		exit (-1);
	}
	printf ("open /dev/ttyAMA2 successed!\n");
	
	//设置波特率、数据位、停止位、检验方式
	uart.baudrate = BAUDRATE; //115200
	uart.nbits = NBITS; //8
	uart.stopbits = STOPBITS; //1
	uart.event = NONE;

	int ret = 0;
	//设置串口的相关属性
	if (0 > (ret = uart_setting (uart_fd, uart))) {
		perror ("uart_setting error");
		exit (-1);
	}
	printf ("uart2 setting successed!\n");
	return ;
}
/***********************************************************************
功能描述：
    当调用这个函数时，首先会读取舵机的软件版本号（这是必要的操作），
之后会检查按键key3的状态，如果key3的状态发生了改变，就会执行下面的操作：
打开用户LED灯-->设置旋转速度-->旋转90度-->关闭LED灯-->旋转回0度。
    如果key3的状态并没有发生改变，则函数调用结束
************************************************************************/
void steer_auto (void)
{
	//读取舵机控制器的软件版本号
	steering_ctrl (uart_fd, buf_steer, VERSION);
	printf ("version:\%d.%d\n\n", buf_steer[4], buf_steer[5]);
	//printf ("please press the key3....\n");
	
	int ret = 0;
	//读取舵机的运动状态，buf[4] << 8 | buf[5]不为0, 就证明舵机可用
	steering_ctrl (uart_fd, buf_steer, BUT_READ);
	ret = buf_steer[4] << 8 | buf_steer[5];
	usleep (10000);
	if (ret != 0) {
		printf ("steering is active...\n");
	
		//设置用户LED灯开
		steering_ctrl (uart_fd, buf_steer, LED_ON);
		usleep (10000);
	
		//设置舵机转速
		steering_ctrl (uart_fd, buf_steer, SPEED7);
		usleep (10000);
	
		//选择舵机旋转角度{
		steering_ctrl (uart_fd, buf_steer, DEGREE90);	
		usleep (10000);

		//等待舵机运动完成并读取舵机的运动状态
		ret = 0xfffc;
		while (ret == 0xfffc) {
			read (uart_fd, buf_steer, 8);
			ret = buf_steer[4] << 8 | buf_steer[5];
			usleep (10000);
		}
		ret = 0xffff;
		while (ret != 0) {
			steering_ctrl (uart_fd, buf_steer, STAT_READ);
			ret = buf_steer[4] << 8 | buf_steer[5];
			usleep (10000);
		}
		usleep (1000000);
		//关闭用户灯
		steering_ctrl (uart_fd, buf_steer, LED_OFF);
	
		//让舵机旋转至0°
		steering_ctrl (uart_fd, buf_steer, DEGREE0);
		usleep (100000);

		//等待舵机运动完成并读取舵机的运动状态
		ret = 0xfffc;
		while (ret == 0xfffc) {
			read (uart_fd, buf_steer, 8);
			ret = buf_steer[4] << 8 | buf_steer[5];
			usleep (10000);
		}
		ret = 0xffff;
		while (ret != 0) {
			steering_ctrl (uart_fd, buf_steer, STAT_READ);
			ret = buf_steer[4] << 8 | buf_steer[5];
			usleep (10000);
		}
	}
}
//舵机旋转90度
void steer_90degree (void)
{
	int ret;
	
	//读取舵机控制器的软件版本号
	steering_ctrl (uart_fd, buf_steer, VERSION);
	//printf ("enter steer_90degree1!\n");
	printf ("version:\%d.%d\n\n", buf_steer[4], buf_steer[5]);
	//printf ("enter steer_90degree2!\n");
	printf ("steering is active...\n");
	
	//设置用户LED灯开
	steering_ctrl (uart_fd, buf_steer, LED_ON);
	usleep (10000);
	
	//设置舵机转速
	steering_ctrl (uart_fd, buf_steer, SPEED7);
	usleep (10000);
	
	//选择舵机旋转角度{
	steering_ctrl (uart_fd, buf_steer, DEGREE90);	
	usleep (10000);

	//等待舵机运动完成并读取舵机的运动状态
	ret = 0xfffc;
	
	while (ret == 0xfffc) {
		read (uart_fd, buf_steer, 8);
		ret = buf_steer[4] << 8 | buf_steer[5];
		usleep (10000);
	}
	ret = 0xffff;
	while (ret != 0) {
		steering_ctrl (uart_fd, buf_steer, STAT_READ);
		ret = buf_steer[4] << 8 | buf_steer[5];
		usleep (10000);
	}
}


//舵机旋转0度
void steer_0degree (void)
{
	int ret;
	//读取舵机控制器的软件版本号
	steering_ctrl (uart_fd, buf_steer, VERSION);
	printf ("version:\%d.%d\n\n", buf_steer[4], buf_steer[5]);
	
	printf ("steering is active...\n");
	
	//设置舵机转速
	steering_ctrl (uart_fd, buf_steer, SPEED7);
	usleep (10000);

	//关闭用户灯
	steering_ctrl (uart_fd, buf_steer, LED_OFF);
	
	//让舵机旋转至0°
	steering_ctrl (uart_fd, buf_steer, DEGREE0);
	usleep (10000);

	//等待舵机运动完成并读取舵机的运动状态
	ret = 0xfffc;
	while (ret == 0xfffc) {
		read (uart_fd, buf_steer, 8);
		ret = buf_steer[4] << 8 | buf_steer[5];
		usleep (10000);
	}
	ret = 0xffff;
	while (ret != 0) {
		steering_ctrl (uart_fd, buf_steer, STAT_READ);
		ret = buf_steer[4] << 8 | buf_steer[5];
		usleep (10000);
	}
}

